#ifndef LIDAR_LOCALIZATION_MODELS_CERES_OPTIMIZER_QUATION_HPP_
#define LIDAR_LOCALIZATION_MODELS_CERES_OPTIMIZER_QUATION_HPP_

#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <deque>
namespace lidar_localization
{
//定义顶点以及边的数据结构 用一个类来管理所有的数据
typedef Eigen::Matrix<double,6,1> Vector6d;
typedef Eigen::Matrix<double,6,6> Matrix6d;

struct cereNode
{
    /* data */
    unsigned int index;
    double t[3];
    double q[4];
    bool need_fix = false;
};
struct cereEdge
{
    unsigned int index1;
    unsigned int index2;
    double t[3] ;
    double q[4];
    Matrix6d conversie;
};

struct cereEdgeXYZ
{
    unsigned int index;
    double t[3];
    Eigen::Matrix3d conversie;
};
//-----------------------------------------------------------------------------------------------------------------
// 这里为工具函数，参考的 VINS—Fusion，初次接触可以暂时跳过，只要知道函数功能即可
template <typename Derived>
static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
{
    typedef typename Derived::Scalar Scalar_t;

    Eigen::Quaternion<Scalar_t> dq;
    Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
    half_theta /= static_cast<Scalar_t>(2.0);
    dq.w() = static_cast<Scalar_t>(1.0);
    dq.x() = half_theta.x();
    dq.y() = half_theta.y();
    dq.z() = half_theta.z();
    return dq;
}

template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 3, 3> skewSymmetric(const Eigen::MatrixBase<Derived> &q)
{
    Eigen::Matrix<typename Derived::Scalar, 3, 3> ans;
    ans << typename Derived::Scalar(0), -q(2), q(1),
            q(2), typename Derived::Scalar(0), -q(0),
            -q(1), q(0), typename Derived::Scalar(0);
    return ans;
}

template <typename Derived>
static Eigen::Quaternion<typename Derived::Scalar> positify(const Eigen::QuaternionBase<Derived> &q)
{
    return q;
}

template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 4, 4> Qleft(const Eigen::QuaternionBase<Derived> &q)
{
    Eigen::Quaternion<typename Derived::Scalar> qq = positify(q);
    Eigen::Matrix<typename Derived::Scalar, 4, 4> ans;
    ans(0, 0) = qq.w(), ans.template block<1, 3>(0, 1) = -qq.vec().transpose();
    ans.template block<3, 1>(1, 0) = qq.vec(), ans.template block<3, 3>(1, 1) = qq.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() + skewSymmetric(qq.vec());
    return ans;
}

template <typename Derived>
static Eigen::Matrix<typename Derived::Scalar, 4, 4> Qright(const Eigen::QuaternionBase<Derived> &p)
{
    Eigen::Quaternion<typename Derived::Scalar> pp = positify(p);
    Eigen::Matrix<typename Derived::Scalar, 4, 4> ans;
    ans(0, 0) = pp.w(), ans.template block<1, 3>(0, 1) = -pp.vec().transpose();
    ans.template block<3, 1>(1, 0) = pp.vec(), ans.template block<3, 3>(1, 1) = pp.w() * Eigen::Matrix<typename Derived::Scalar, 3, 3>::Identity() - skewSymmetric(pp.vec());
    return ans;
}
void Convert2qt(Eigen::Quaterniond &q, Eigen::Vector3d &t, double q_[4], double t_[3]);

class PoseLocalParameterization : public ceres::LocalParameterization
{
    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const{
        Eigen::Map<const Eigen::Quaterniond> _q(x);
        Eigen::Quaterniond dq = deltaQ(Eigen::Map<const Eigen::Vector3d>(delta));
        Eigen::Map<Eigen::Quaterniond> q(x_plus_delta);
        q = (_q * dq).normalized();

        return true;
    }

    virtual bool ComputeJacobian(const double *x, double *jacobian) const{
        Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> j(jacobian);
        j.topRows<3>().setIdentity();
        j.bottomRows<1>().setZero();
        return true;
    }
    virtual int GlobalSize() const { return 4; };
    virtual int LocalSize() const { return 3; };
};
using namespace Eigen;
//位置图相关的 CostFunction
class PoseGraphCostFunction: public ceres::SizedCostFunction<6, 3, 4, 3, 4>{
public:
    PoseGraphCostFunction(Quaterniond q_m, Vector3d t_m, Matrix6d cov): q_m(q_m), t_m(t_m), covariance(cov){}

    virtual bool Evaluate(double const* const* parameters, double *residuals, double **jacobians) const{
        Vector3d t1(parameters[0][0], parameters[0][1], parameters[0][2]);
        Quaterniond q1(parameters[1][3], parameters[1][0], parameters[1][1], parameters[1][2]);

        Vector3d t2(parameters[2][0], parameters[2][1], parameters[2][2]);
        Quaterniond q2(parameters[3][3], parameters[3][0], parameters[3][1], parameters[3][2]);

        Eigen::Map<Matrix<double, 6, 1>> residual(residuals);

        residual.segment(0, 3) = q1.inverse() * (t2 - t1) - t_m;
        residual.segment(3, 3) = 2 * (q_m.inverse() * (q1.inverse() * q2)).vec();

        if(jacobians){
            if(jacobians[0]){
                Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>> jacobian(jacobians[0]);
                jacobian.block<3, 3>(0, 0) = -q1.inverse().toRotationMatrix();
                jacobian.block<3, 3>(3, 0) = Matrix3d::Zero();
            }

            if(jacobians[1]){
                Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>> jacobian(jacobians[1]);
                jacobian.setZero();
                jacobian.block<3, 3>(0, 0) = skewSymmetric(q1.inverse() * (t2 - t1));
                jacobian.block<3, 3>(3, 0) = -(Qright(q1.inverse() * q2) * Qleft(q_m.inverse())).bottomRightCorner<3, 3>();
//                jacobian.block<3, 3>(3, 0) = -0.5*(Qright(q2) * Qleft(q_m.inverse() * q1.inverse())).bottomRightCorner<3, 3>();
            }

            if(jacobians[2]){
                Eigen::Map<Eigen::Matrix<double, 6, 3, Eigen::RowMajor>> jacobian(jacobians[2]);
                jacobian.block<3, 3>(0, 0) = q1.inverse().toRotationMatrix();
                jacobian.block<3, 3>(3, 0) = Matrix<double, 3, 3>::Zero();
            }

            if(jacobians[3]){
                Eigen::Map<Eigen::Matrix<double, 6, 4, Eigen::RowMajor>> jacobian(jacobians[3]);
                jacobian.setZero();
                jacobian.block<3, 3>(3, 0) = Qleft(q_m.inverse() * q1.inverse() * q2).bottomRightCorner<3, 3>();
//                jacobian.block<3, 3>(3, 0) = 0.5*(Qright(q2) * Qleft(q_m.inverse() * q1.inverse())).bottomRightCorner<3, 3>();
            }
        }
        return true;
    }

private:
    Quaterniond q_m;
    Vector3d t_m;
    Matrix6d covariance;
};

class PoseGraphCostFunctionXYZ : public ceres::SizedCostFunction<3,3>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual ~PoseGraphCostFunctionXYZ(){}
    PoseGraphCostFunctionXYZ(Eigen::Vector3d _se3, Eigen::Matrix3d _covariance): measurment_se3(_se3), convariance(_covariance){}

    virtual bool Evaluate(double const* const* parameters,
                          double *residuals,
                          double **jacobians) const{

        Eigen::Map<const Eigen::Vector3d> pose_i_trans(parameters[0]);
        Eigen::Matrix3d sqrt_info = convariance;

        Eigen::Vector3d residual =   (measurment_se3 - pose_i_trans);
        residuals[0] = residual.x();
        residuals[1] = residual.y();
        residuals[2] = residual.z();
        if(jacobians){
            if(jacobians[0]) {
                Eigen::Map<Eigen::Matrix<double,3,3>> jacobian_i(jacobians[0]);
                jacobian_i.setIdentity();
                jacobian_i = -1 * jacobian_i;
            }
        }
        return true;
    }
private:
    const Eigen::Vector3d measurment_se3;
    const Eigen::Matrix3d convariance;
};






class CeresGraphOptimizer {
public:
    CeresGraphOptimizer(){};
    // 优化
    bool Optimize() ;
    // 输入、输出数据
    bool GetOptimizedPose(std::deque<Eigen::Matrix4f>& optimized_pose) ;
    int GetNodeNum() ;
    // 添加节点、边、鲁棒核
    // void SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size){}
    void AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix) ;
    void AddSe3Edge(int vertex_index1,
                            int vertex_index2,
                            const Eigen::Isometry3d &relative_pose,
                            Vector6d noise);
    void AddSe3PriorXYZEdge(int se3_vertex_index,
                                    const Eigen::Vector3d &xyz,
                                    Eigen::Vector3d noise);
    void AddSe3PriorQuaternionEdge(int se3_vertex_index,
                                           const Eigen::Quaterniond &quat,
                                           Eigen::VectorXd noise);
    // 设置优化参数
    void SetMaxIterationsNum(int max_iterations_num);
private:
    std::vector<cereNode> nodeDataBuff;
    std::vector<cereEdge> edgeDataBuff;
    std::vector<cereEdgeXYZ> edgeXYZDatabuff;
};
}








#endif